/**
  ******************************************************************************
  * File Name          : LEG.c
  * Description        : �Ȳ��˶��Ķ��ʵ�ּ������ʼ��.
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2019 Little-Doggy.
  * All rights reserved.</center></h2>
  *
  ******************************************************************************
  */

#include "leg.h"
#include "SCServo.h"
#include "scs_uart.h"
#include "sensor_task.h"

/**
  * @brief ���ڴ洢������λ�õ�double����
	* @attention 
	* ֻ�ܷŵ�.c��ŵ�.h�ᱨ�����£�
	* Error: L6200E: Symbol zero_pos multiply defined (by control_task.o and leg.o).
	* Error: L6200E: Symbol zero_pos multiply defined (by main.o and leg.o).
  */
double zero_pos[13];

Leg_t LF = LEG_DEFAULT, RF = LEG_DEFAULT, LB = LEG_DEFAULT, RB = LEG_DEFAULT;


/**
  * @brief  ���ö�������λ��
  * @retval None
  */
void Set_ZeroPos()
{
	zero_pos[RF_SERVO_1] = 0;
	zero_pos[RF_SERVO_2] = 0;
	zero_pos[RF_SERVO_3] = 0;
	zero_pos[LF_SERVO_1] = -20;
	zero_pos[LF_SERVO_2] = 15;
	zero_pos[LF_SERVO_3] = 33;
	
	zero_pos[RB_SERVO_1] = 33;
	zero_pos[RB_SERVO_2] = 33;
	zero_pos[RB_SERVO_3] = -33;

	zero_pos[LB_SERVO_1] = -10;
	zero_pos[LB_SERVO_2] = 15;
	zero_pos[LB_SERVO_3] = 33;
	//��ʼ��λ��LBΪ180 ����200
}

/**
  * @brief  �����ʼ��
  * @retval None
  */
void Leg_Init(void)
{
	Set_ZeroPos();
	
	LF.index = LF_LEG;
	RF.index = RF_LEG;
	LB.index = LB_LEG;
	RB.index = RB_LEG;
	
//	LF.zero_y = 10.7 + -1;
//	LB.zero_y	= 10.5 + -1;
//	RB.zero_y	= 10.2 + -1;
//	RF.zero_y	= 10.4 + -1;
	
}

/**
  * @brief  ���ö��ת���Ƕ�
  * @param  servo: ѡ����Ҫ���ƵĶ��
  * @param  degree: �趨�Ķ��ת���Ƕ�
  * @retval None
  */
void Set_Servo_Angle(Servo servo, double degree)
{
	uint16_t pos = 512 + degree/PI * 2 * (815-512)+ zero_pos[servo];
	WritePos(servo, pos, 0, 0);
}

/**
  * @brief  ����Leg_Indexת��
  * @param  leg: ѡ����Ҫ���Ƶ�Leg_Index
  * @param  degree1: Leg_Index�Ķ��1��ת��
  * @param  degree2: Leg_Index�Ķ��2��ת��
  * @retval None
  */
void Set_Leg_Angle(Leg_Index leg, double degree1, double degree2, double degree3)
{
	
		
	if(leg == LF.index || leg == RB.index) {
		degree2 *=-1;
		degree3 *=-1;
	}
	if(leg == LB.index || leg == RB.index) 
		degree1 *=-1;
	Set_Servo_Angle((Servo)((int)leg*3+1), __LIMIT(degree1, -PI/6, PI / 6));
	Set_Servo_Angle((Servo)((int)leg*3+2), __LIMIT(degree2, -PI/3, PI / 3));
	Set_Servo_Angle((Servo)((int)leg*3+3), __LIMIT(degree3, -PI*5/6, PI*5 / 6));
}

/**
  * @brief  ��̬���㺯������(x, y)����Ϊ��Ӧ���ת��(alpha, theta)
  * @param  leg: ��Ҫ������̬�����Leg_Index
  * @param  x: ����x����
  * @param  y: ����y����
  * @retval None
  */
void Set_Feet_Pos(Leg_Index leg, double x, double y, double z)
{
	double a = 6, b = 5.5;
	double alpha, theta, gama, sqr_sum;
	if(leg == LF.index || leg == RF.index) 
		x *=-1;
	sqr_sum = x*x + y*y + z*z;
	theta = acos((sqr_sum - a*a-b*b)/2/a/b);
	alpha = asin(b*sin(theta)/sqrt(sqr_sum)) + atan(x/sqrt(y*y+z*z));
	gama = atan(z/y);
  
	Set_Leg_Angle(leg, gama, alpha, theta);
}


Touch_State Feet_Get_TouchState(Leg_Index leg_index)
{
	switch (leg_index)
	{
	case RF_LEG:
		if (torque[leg_index*3+3] > 270) return OVER_TOUCH;
		if (torque[leg_index*3+3] > 230) return MOVE_TOUCH;
		else if (torque[leg_index*3+3] > 80) return STATIC_TOUCH;
		else return NONE_TOUCH;
	case LF_LEG:
		if(torque[leg_index*3+3] > 600) return STATIC_TOUCH;
		if (torque[leg_index*3+3] < -250) return OVER_TOUCH;
		if (torque[leg_index*3+3] < -230) return MOVE_TOUCH;
		else if (torque[leg_index*3+3] < -80) return STATIC_TOUCH;
		else return NONE_TOUCH;
		break;
	case LB_LEG:
		return torque[leg_index*3+3] > 80;
		break;
	case RB_LEG:
		return torque[leg_index*3+3] < -70;
		break;
	default:
		return STATIC_TOUCH;
		break;
	}
}


